#pragma once
#include "Pose3D.h"
#include <Eigen/Eigen>
#include <vector>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/kdtree/kdtree_flann.h>
using namespace std;
using namespace pcl;
typedef pcl::PointXYZ PointIoType;


class PoseGrabber
{
public:
	vector<Eigen::MatrixX4d> transfromMatrix;
	vector<Pose3D> ground_truth;
	vector<Pose3D>& getGroudTruth();
	void getGroudTruthWithNoisy(vector<Pose3D>& GT_noisy,float noisy=1.0);
	bool PoseDataInit(const string& gound_truth_path);
	void readGroudTruthOffLine(const string& gound_truth_path);
	void get(const int index);
	void setPoseAsPointCloud();
	pcl::PointCloud<pcl::PointXYZ>::Ptr getPoseAsPointCloud();
	void clear();
	PoseGrabber();
	~PoseGrabber();
private:
	
	pcl::PointCloud<PointIoType>::Ptr mTruth_pose_cloud = pcl::PointCloud<PointIoType>::Ptr(new pcl::PointCloud<PointIoType>);
};

